/*
 * Motor.cpp
 *
 *  Created on: Apr 20, 2010
 *      Author: Soheil Damangir
 */

#include "rrcon.h"

#include "Motor.h"
#include "../Util/Conf.h"
#include "../Util/ConfigFile.h"

using namespace kthrobot;
namespace Group5 {

Motor::Motor(const string& _name, int _index): name(_name), index(_index) {
  ConfigFile cf = Conf::get();
  pid = new PID_controller(cf.read<int>(name+"_Kp"), cf.read<int>(name+"_Ti"), cf.read<int>(name+"_Td"), cf.read<int>(name+"_dt", cf.read<int>("dt")));
  current_speed_signal = 0;
  handler = RRCon::motor(index);
  dead_band = cf.read<int>(name+"_dead");
}
inline void Motor::resetDist()
{
  handler.SetEncToZero();
}
inline int Motor::getDist()
{
  return handler.ReadEnc().cur * Constants::get()->ENC_TO_MM;
}
void Motor::setSpeed(int _speed)
{
  pid->setPoint(speed);
}
int Motor::getSpeed()
{
  EncoderData enc = handler.ReadEnc();
  return (enc.cur - enc.prev)*Constants::get()->ENC_TO_MM*Constants::get()->ENC_UD_FREQ;
}
void Motor::TaskCode()
{
  current_speed_signal = pid->next(getSpeed());
  if (current_speed_signal > 0){
    if (current_speed_signal < dead_band)
      current_speed_signal = dead_band;
    if (current_speed_signal > handler.pwm_abs_max)
      current_speed_signal = handler.pwm_abs_max;
  }else{
    if (current_speed_signal > -dead_band)
      current_speed_signal = -dead_band;
    if (current_speed_signal < -handler.pwm_abs_max)
      current_speed_signal = -handler.pwm_abs_max;
  }
  handler.SetSpeed(next);
  WaitPeriod();
}
Motor::~Motor() {
  Join();
  delete pid;
}

}
